/**
 * IK Arm
 * Based on Reach 1. 
 * Based on code from Keith Peters (www.bit-101.com)
 * 
 * The arm follows the position of the mouse by
 * calculating the angles with atan2(). 
 */

import processing.serial.*;

// String portname = "/dev/tty.usbserial-A5002rKW"; // james arduino
 String portname = "/dev/tty.usbserial-A1001NQr"; // thomas' arduino, w/ feet
// String portname = "/dev/tty.SerialPort-1"; // for testing
Serial port;  // Create object from Serial class


float x = 100;
float y = 100;
float x2 = 100;
float y2 = 100;
float segLength = 50;


float xFakMauxStart = 120;  // for moving w/o mouse
float xFakMaux = xFakMauxStart;  // for moving w/o mouse
float FakStart = 0;
float FakMax = 70;
float yFakMaux = FakStart;
int FakCount = 0;
int FakCountMax = 6;

void setup() {
  size(200, 300);
  smooth(); 
  strokeWeight(1.0);
  stroke(0, 100);
  PFont fontA = loadFont("Ziggurat-HTF-Black-32.vlw");
  textFont(fontA, 12);
  
  // init serial
  port = new Serial(this, portname, 9600);

}

// servo 0, rotation of base
String angle2Servo0(float a) { // a = angle in radians    
   int center = 2830; // centered rotation
   int c90 = 4520; // 90deg to the right, clockwise
   int cc90 = 1140; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s0" + nf(value, 4); 
}

// servo 1, elevation of 1st arm segment from base, shoulder
String angle2Servo1(float a) { // a = angle in radians    
   int center = 1280; // parallel with the ground plane
   int c90 = 3030; // perpendicular to the ground plane
   int value = center;  // if below is not met, a = 0 exactly
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
  return "s1" + nf(value, 4); 
}

// servo 2, elevation of 2nd arm segment from base, elbow
String angle2Servo2(float a) { // a = angle in radians    
   int center = 3210; // centered rotation
   int c90 = 4860; // 90deg to the right, clockwise
   int cc90 = 1560; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s2" + nf(value, 4); 
}


void draw() {
  background(200);
  float dx = xFakMaux - x; // x = 100, 200x200 sketch, change the coord. system
  float dy = yFakMaux - y;
  float angle1 = atan2(dy, dx);   // calculates in radians pi to - pi
  
  float tx = xFakMaux - cos(angle1) * segLength;
  float ty = yFakMaux - sin(angle1) * segLength;
  dx = tx - x2;  // x2 is the center of the matrix
  dy = ty - y2;
  float angle2 = atan2(dy, dx);  
  x = x2 + cos(angle2) * segLength;
  y = y2 + sin(angle2) * segLength;
  
  segment(x, y, angle1); 
  segment(x2, y2, angle2); 
  
  // output angles
  fill(100);
  text("angle1=" + degrees(angle1), 50, 120);
  text("angle2=" + degrees(angle2), 50, 135);
  text("dx=" + dx, 50, 150);
  text("dy=" + dy, 50, 165);
  text("xFakMaux=" + xFakMaux, 50, 180);
  text("yFakMaux=" + yFakMaux, 50, 195);


  
  
  int servo190 = 3030; // servo1 at 90    angle 0 in calc
  int servo100 = 1280; // servo1 at 0 
  int servo290 = 4860; // servo2 at 90 down  180 deg in calc
  int servo200 = 3210; // level with other seg
  int servo2i90 = 1560; // -90 up
  
/*
text("AngleOnly" + (( -1 * (degrees(angle2) - degrees(angle1)))) , 50, 215);
    text("calcAngle" + nf(((int)((servo290 - servo200) / 90 * -1 * (degrees(angle2) - degrees(angle1))) + servo200), 4) , 50, 245);
*/  

  yFakMaux+= .5;
  xFakMaux += 0.3/2;
/*
// if (FakCount == 1 && yFakMaux == FakStart) {
    delay(500);
 //   port.write("s02800   s23500    s33140     ");
    delay(500);
 //   port.write("s02800   s13500    s33140     ");
 //   delay(500);

  } */ 
 
  if (yFakMaux >= FakMax) {
      yFakMaux = FakStart;
      xFakMaux = xFakMauxStart; // reset variable
       if (3 == 3){
 //   delay(1000);
 //   port.write("s02800   s22000    s33140     ");
    delay(1000);
    port.write("s02800   s14000    s33140     ");
    delay(1000);

       }  
      FakCount++;
      
      if (FakCount == 4) {
          FakCount = 0;
      }
  }  else if (yFakMaux <= 25 + 10 && yFakMaux > 25 + 5) {  // knife in
        xFakMaux+= 1.25;
      } else if (yFakMaux >= 70 - 10 && yFakMaux < 70 - 5) {  // knife out
        xFakMaux-= 1.25;
      } 

      if (yFakMaux >= FakStart + 2){
  //      port.write("s02800       s33140     ");
        port.write("s1" + nf((int)(servo100 + (servo190 - servo100) / 90 * -1 * degrees(angle2)), 4));
        port.write("s2" + nf(((int)((servo290 - servo200) / 90 * -1 * (degrees(angle2) - degrees(angle1))) + servo2i90), 4));
      }
  line(155, 100, 155, 0); // draw line for comparison
//  line(xFakMaux, 100, xFakMaux, 0); // draw line for comparison

  delay(100); // slow to visualize


}

void segment(float x, float y, float a) {
  pushMatrix();
  translate(x, y); // redefine the origin
  rotate(a);       // rotate in radians
  line(0, 0, segLength, 0);  // draw line the seglength
  popMatrix();
}

